#include "MarchingHeap.h"
#include "Utility.h"

bool operator< (const LevelSetWithIndex& lfs, const LevelSetWithIndex& rhs)
{
	return lfs.m_phi < rhs.m_phi; 
}
bool operator> (const LevelSetWithIndex& lfs, const LevelSetWithIndex& rhs)
{
	return lfs.m_phi > rhs.m_phi; 
}

bool operator< (const Index3& lfs, const Index3& rhs)
{
	return (lfs.m_i <  rhs.m_i &&  lfs.m_j < rhs.m_j && lfs.m_k < rhs.m_k); 
}

bool operator!= (const Index3& lfs, const Index3& rhs)
{
	return (lfs.m_i !=  rhs.m_i ||  lfs.m_j != rhs.m_j || lfs.m_k != rhs.m_k); 
}


DecoArchive& operator<< (DecoArchive& Ar, const Index3& idx)
{
	return Ar << idx.m_i << idx.m_j << idx.m_k;
}
DecoArchive& operator>> (DecoArchive& Ar, Index3& idx)
{
	return Ar >> idx.m_i >> idx.m_j >> idx.m_k;
}

MarchingHeap::MarchingHeap() : m_numX(0), m_numY(0), m_numZ(0), m_indices(NULL)
{

}

MarchingHeap::MarchingHeap(UINT numX, UINT numY, UINT numZ) : m_numX(numX), m_numY(numY), m_numZ(numZ)
{
	assert(numX && numY && numZ);
	m_indices = Malloc3DArray<INT>(numX, numY, numZ);
	for (UINT i = 0; i < numX; i++)
	{
		for (UINT j = 0; j < numY; j++)
		{
			for (UINT k = 0; k < numZ; k++)
			{
				m_indices[i][j][k] = -1;
			}
		}
	}
}
MarchingHeap::~MarchingHeap(void)
{
	if (m_indices)
		Free3DArray(m_indices, m_numX, m_numY, m_numZ);
	m_numX = m_numY = m_numZ = 0;
	dist.clear();
	indexInHeap.clear();
}

void MarchingHeap::setSize(UINT numX, UINT numY, UINT numZ)
{
	assert(numX && numY && numZ);
	if (m_indices)
		Free3DArray<INT>(m_indices, m_numX, m_numY, m_numZ);
	m_numX = numX;
	m_numY = numY;
	m_numZ = numZ;
	m_indices = Malloc3DArray<INT>(numX, numY, numZ);
	for (UINT i = 0; i < numX; i++)
	{
		for (UINT j = 0; j < numY; j++)
		{
			for (UINT k = 0; k < numZ; k++)
			{
				m_indices[i][j][k] = -1;
			}
		}
	}
}

void MarchingHeap::clear()
{
	for (UINT i = 0; i < m_numX; i++)
	{
		for (UINT j = 0; j < m_numY; j++)
		{
			for (UINT k = 0; k < m_numZ; k++)
			{
				m_indices[i][j][k] = -1;
			}
		}
	}
	dist.clear();
	indexInHeap.clear();
}

LevelSetWithIndex MarchingHeap::pop (void)
{
	size_t sz = size();
	assert(sz >= 1);
	LevelSetWithIndex result(dist[0], indexInHeap[0].m_i, indexInHeap[0].m_j, indexInHeap[0].m_k);
	exchange(0, static_cast<UINT>(sz - 1));
	m_indices[indexInHeap[sz - 1].m_i][indexInHeap[sz - 1].m_j][indexInHeap[sz - 1].m_k] = -1;
	dist.pop_back();
	indexInHeap.pop_back();

	minHeapify(0);
	return result;
}
void MarchingHeap::push (const LevelSetWithIndex& elem)
{
	//assert(elem.m_phi >= 0);
	UINT index = findIndex(elem.m_i, elem.m_j, elem.m_k);
	if (index == -1)
	{
		dist.push_back(MAX_DOUBLE);
		indexInHeap.push_back(Index3(elem.m_i, elem.m_j, elem.m_k));
		size_t sz = size();
		m_indices[indexInHeap[sz - 1].m_i][indexInHeap[sz - 1].m_j][indexInHeap[sz - 1].m_k] = static_cast<UINT>(sz - 1);
		heapDecreaseKey(size() - 1, elem.m_phi);
	}
	else
	{
		heapDecreaseKey(index, elem.m_phi);
	}
}

void MarchingHeap::heapDecreaseKey(UINT i, DOUBLE value)
{
	if (signedAbs(value) > signedAbs(dist[i]))
		return;
	dist[i] = value;
	while (i > 0 && signedAbs(dist[parent(i)]) > signedAbs(dist[i]))
	{
		exchange(i, parent(i));
		i = parent(i);
	}
}

void MarchingHeap::minHeapify(UINT i)
{
	UINT l = left(i);
	UINT r = right(i);
	UINT smallest = -1;
	if (l < size() && signedAbs(dist[l]) < signedAbs(dist[i]))
	{
		smallest = l;
	}
	else
	{
		smallest = i;
	}
	if (r < size() && signedAbs(dist[r]) < signedAbs(dist[smallest]))
		smallest = r;
	if (smallest != i)
	{
		exchange(i, smallest);
		minHeapify(smallest);
	}

}

UINT MarchingHeap::findIndex(INT i, INT j, INT k)
{
	//size_t sz = indexInHeap.size();
	//assert(sz == dist.size());
	//for (size_t it = 0; it < sz; it++)
	//{
	//	if (i == indexInHeap[it].m_i && j == indexInHeap[it].m_j && k == indexInHeap[it].m_k)
	//	{
	//		return static_cast<UINT>(it);
	//	}
	//}
	//return -1;
	return m_indices[i][j][k];
}